/*=========================================================================
This file is part of CustusX, an Image Guided Therapy Application.

Copyright (c) SINTEF Department of Medical Technology.
All rights reserved.

CustusX is released under a BSD 3-Clause license.

See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
=========================================================================*/

#include "cxToolTipCalibrationWidget.h"

#include <QPushButton>
#include <QTextStream>
#include <QFileDialog>
#include <QMessageBox>
#include "cxTypeConversions.h"
#include "cxLogger.h"
#include "cxTrackingService.h"
#include "cxVector3D.h"
#include "cxLabeledComboBoxWidget.h"
#include "cxTool.h"
#include "cxVisServices.h"
#include "cxStringPropertySelectTool.h"
#include "cxSpaceProvider.h"

namespace cx
{

//------------------------------------------------------------------------------
ToolTipCalibrateWidget::ToolTipCalibrateWidget(VisServicesPtr services, QWidget* parent) :
	BaseWidget(parent, "tool_tip_calibrate_widget", "ToolTip Calibrate"),
	mServices(services),
	mCalibrateButton(new QPushButton("Calibrate")),
	mReferencePointLabel(new QLabel("Ref. point:")),
	mTestButton(new QPushButton("Test calibration")),
	mCalibrationLabel(new QLabel("Calibration: \n")),
	mDeltaLabel(new QLabel("Delta:"))
{
	QVBoxLayout* toplayout = new QVBoxLayout(this);

	mTools = StringPropertySelectTool::New(mServices->tracking());
	mTools->setValueName("Reference tool");
	mTools->setHelp("Select a tool with a known reference point");
	mCalibrateToolComboBox = new LabeledComboBoxWidget(this, mTools);
	this->setToolTip("Calibrate tool position part of sMt matrix");

	//toplayout->addWidget(new QLabel("<b>Select a tool with a known reference point:</b>"));
	toplayout->addWidget(mCalibrateToolComboBox);
	toplayout->addWidget(mReferencePointLabel);
	toplayout->addWidget(mCalibrateButton);
	toplayout->addWidget(mCalibrationLabel);
	toplayout->addWidget(this->createHorizontalLine());
	toplayout->addWidget(mTestButton);
	toplayout->addWidget(mDeltaLabel);
	toplayout->addStretch();

	connect(mCalibrateButton, SIGNAL(clicked()), this, SLOT(calibrateSlot()));
	connect(mTestButton, SIGNAL(clicked()), this, SLOT(testCalibrationSlot()));

	connect(mTools.get(), SIGNAL(changed()), this, SLOT(toolSelectedSlot()));
	connect(mServices->tracking().get(), &TrackingService::stateChanged, this, &ToolTipCalibrateWidget::onTrackingSystemStateChanged);

	//setting default state
	this->toolSelectedSlot();
}

ToolTipCalibrateWidget::~ToolTipCalibrateWidget()
{}

void ToolTipCalibrateWidget::onTrackingSystemStateChanged()
{
	if (mServices->tracking()->getTool(mTools->getValue()))
		return;
	if (!mServices->tracking()->getReferenceTool())
		return;

	mTools->setValue(mServices->tracking()->getReferenceTool()->getUid());
}

void ToolTipCalibrateWidget::calibrateSlot()
{
	ToolPtr refTool = mTools->getTool();
	//Todo, we only allow the reference point with id 1 to be used to calibrate
	//this could be done more dynamic.
	if(!refTool || !refTool->hasReferencePointWithId("1"))
		return;

	ToolPtr tool = mServices->tracking()->getActiveTool();
	CoordinateSystem to = mServices->spaceProvider()->getT(tool);
	Vector3D P_t = mServices->spaceProvider()->getActiveToolTipPoint(to);

	ToolTipCalibrationCalculator calc(mServices->spaceProvider(), tool, refTool, P_t);
	Transform3D calibration = calc.get_calibration_sMt();

	QMessageBox msgBox;
	msgBox.setText("Do you want to overwrite "+tool->getName()+"s calibration file?");
	msgBox.setInformativeText("This cannot be undone.");
	msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel);
	msgBox.setDefaultButton(QMessageBox::Ok);
	int ret = msgBox.exec();

	if(ret == QMessageBox::Ok)
	{
		try
		{
			tool->setCalibration_sMt(calibration);
		}
		catch(std::exception& e)
		{
			QMessageBox msgBox2;
			msgBox2.setText("Unknown error, could not calibrate the tool: "+tool->getName()+".");
			msgBox2.setInformativeText(QString(e.what()));
			msgBox2.setStandardButtons(QMessageBox::Ok);
			msgBox2.setDefaultButton(QMessageBox::Ok);
			int ret2 = msgBox2.exec();
			return;
		}
		mCalibrationLabel->setText("Calibration:\n"+qstring_cast(calibration));
	}
}

void ToolTipCalibrateWidget::testCalibrationSlot()
{
	ToolPtr selectedTool = mTools->getTool();
	if(!selectedTool || !selectedTool->hasReferencePointWithId("1"))
		return;

	CoordinateSystem to = mServices->spaceProvider()->getT(mServices->tracking()->getActiveTool());
	Vector3D sampledPoint = mServices->spaceProvider()->getActiveToolTipPoint(to);

	ToolTipCalibrationCalculator calc(mServices->spaceProvider(), mServices->tracking()->getActiveTool(), selectedTool, sampledPoint);
	Vector3D delta_selectedTool = calc.get_delta_ref();

	mDeltaLabel->setText("<b>Delta:</b> "+qstring_cast(delta_selectedTool)+" <br> <b>Length:</b>  "+qstring_cast(delta_selectedTool.length()));

	report("Delta: "+qstring_cast(delta_selectedTool)+" Length:   "+qstring_cast(delta_selectedTool.length()));
}

void ToolTipCalibrateWidget::toolSelectedSlot()
{
	QString text("Ref. point: <UNDEFINED POINT>");
	mCalibrateButton->setEnabled(false);

	if(mTools->getTool())
	{
		ToolPtr tool = mTools->getTool();
		if(tool && tool->hasReferencePointWithId("1"))
		{
			text = "Ref. point: "+qstring_cast(tool->getReferencePoints()["1"]);
			mCalibrateButton->setEnabled(true);
		}
		else
			reportWarning("Selected tool have no known reference point");
		if(tool)
		{
			mCalibrationLabel->setText("Calibration:\n"+qstring_cast(tool->getCalibration_sMt()));
		}
	}

	mReferencePointLabel->setText(text);
}
//------------------------------------------------------------------------------



ToolTipCalibrationCalculator::ToolTipCalibrationCalculator(SpaceProviderPtr spaces, ToolPtr tool, ToolPtr ref, Vector3D p_t) :
	mTool(tool), mRef(ref), mP_t(p_t), mSpaces(spaces)
{}

ToolTipCalibrationCalculator::~ToolTipCalibrationCalculator()
{}

Vector3D ToolTipCalibrationCalculator::get_delta_ref()
{
	return get_referencePoint_ref() - get_sampledPoint_ref();
}

Transform3D ToolTipCalibrationCalculator::get_calibration_sMt()
{
	return this->get_sMt_new();
}

Vector3D ToolTipCalibrationCalculator::get_sampledPoint_t()
{
	return mP_t;
}

Vector3D ToolTipCalibrationCalculator::get_sampledPoint_ref()
{
	CoordinateSystem csT = mSpaces->getT(mTool); //from
	CoordinateSystem csRef = mSpaces->getT(mRef); //to

	Transform3D refMt = mSpaces->get_toMfrom(csT, csRef);

	Vector3D P_ref = refMt.coord(mP_t);

	return P_ref;
}

Vector3D ToolTipCalibrationCalculator::get_referencePoint_ref()
{
	return mRef->getReferencePoints()["1"];
}

Transform3D ToolTipCalibrationCalculator::get_sMt_new()
{
	Transform3D sMt_old = mTool->getCalibration_sMt();

	CoordinateSystem csT = mSpaces->getT(mTool); //to
	CoordinateSystem csRef = mSpaces->getT(mRef); //from
	Transform3D tMref = mSpaces->get_toMfrom(csRef, csT);

	Vector3D delta_t = tMref.vector(this->get_delta_ref());
	Transform3D T_delta_t = createTransformTranslate(delta_t);

	return sMt_old * T_delta_t;
}
//------------------------------------------------------------------------------
//------------------------------------------------------------------------------
}//namespace cx
